/*
 * TITLE:        main.c
 *
 * PURPOSE:      This is the main entry point for the smallsize GUI
 *               for 2002. The GUI was generated initially using the 
 *               glade program and should be used to update the GUI
 *               appearance. Call it using "glade gui.glade" in the
 *               gui directory. The files in this module include
 *               main.cc         - the main program (symbolic linked to main.c)
 *               callbacks.cc    - the callback functions to drive the program
 *               support.h       - glade utility functions
 *    
 *               Do not edit inteface.* or support.* 
 *               
 * WRITTEN BY:   Brett Browning, some code ported from small size 2001 GUI
 *               written by Michael Bowling
 * (C) Carnegie Mellon University
 *
 */

/*
 * Initial main.c file generated by Glade. Edit as required.
 * Glade will not overwrite this file.
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <gtk/gtk.h>
#include <stdio.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/time.h>

#include <getopt.h>

#include "constants.h"
#include "socket.h"
#include "reality/net_radio.h"
#include "reality/net_vision.h"
#include "soccer/net_gui.h"

#include "reality/client/client.h"

#include "configreader.h"
#include "../utils/stats.h"


#include "interface.h"
#include "support.h"
#include "gtkutils.h"
#include "field.h"
#include "hooks.h"

#include "robotdlg.h"
#include "main.h"

// temp
#include "soccer/modeller.h"


//#define DEBUG


/**************************** TYPES ******************************************/

// macro to pic their team
#define THEIRTEAM(ot)       (((ot) == TEAM_BLUE) ? TEAM_YELLOW : TEAM_BLUE)

#define CHOOSE_COVER_TYPE(c)    ((c) ? VCOVER_NORMAL : VCOVER_NONE)

// number of our robots
#define NUM_OUR_ROBOTS     9

// max value for host name
#define MAX_HOST    256
#define MAX_PATH    256


/**************************** GLOBALS ****************************************/
// file names and host connections
char vision_hostname[MAX_HOST] = "localhost";
char soccer_hostname[MAX_HOST] = "localhost";

Client client;
Socket soccer_s(NET_GUI_PROTOCOL, NET_GUI_ACK_PERIOD);
int vision_input_id = 0;
int soccer_input_id = 0;

// game stats
FrameStats framestats(1.0, true);

Hooks debughooks, framehooks;

static bool running = true;

Field field;
net_vframe frame;
bool selection_frozen = false;
int selection_button = 0;

// widget handles
GtkWidget *statusbar;
GtkWidget *wyellow[MAX_ROBOT_ID_OURS];
GtkWidget *wblue[MAX_ROBOT_ID_OURS];
GtkWidget *mainwin, *aboutdlg;
GtkWidget *gameinfo_entry, *statsinfo_entry;
GtkWidget *refboxwin = NULL, *tacticcombo;
RobotDialog robotdlg[NUM_TEAMS][MAX_TEAM_ROBOTS];
StrategyDialog strategydlg;

bool enable_ids[NUM_TEAMS][MAX_ROBOT_ID];
int map_id2index[NUM_TEAMS][MAX_ROBOT_ID];
int map_index2id[NUM_TEAMS][MAX_TEAM_ROBOTS];
 
bool covers[NUM_TEAMS], enable[NUM_TEAMS];
bool configure = false;

GraphDlgs graphdlgs;
Modeller modeller;


/* strings for converting robot types into a printable message */
const char *rtype_string[] = {"NONE", "DIFF ", "OMNI", "GOAL"};

/*
 * tactic and teamplay strings. Add the tactic  name that you want to appear in the
 * combo box. Make sure the list is terminated with a "\0" string. 
 */
const char *tactic_strings[] = {
  "None", 
  "goalie",
  "shoot", "pass", "dribble_to_shoot",
  "receive_pass", "receive_deflection",
  "steal", "clear", "active_def",
  "block", "mark", "defend_line", "defend_point", "defend_lane", 
  "position_for_loose_ball",
  "position_for_deflection",
  "position_for_rebound",
  "position_for_pass",
  "position_for_kick",
  "dribble_to_position", "position", "stop",
  "charge_ball",
  "latency", 
  "test_step", "test_ramp", "test_kick", "\0\0"
};

const char *team_strings[] = {
  "warmup","teamplay","\0\0"
};

// config constants
CR_DECLARE(STRATEGY_DISPLAY_LENGTH);


/***************************** PROTOTYPES ************************************/
void UpdateInfoBox(net_vframe &f);
gint do_refresh(void);
gint do_connect(void);
void do_disconnect(void);
void do_vision_recv(void);
void do_soccer_recv(void);
void do_configure(net_vconfig &vc);
gint do_radio_connect(void);
gint do_radio_disconnect(void);
void do_radio_control(char cmd);

void UpdateMessage(net_gdebug *debug);
void DrawVisionInfo(net_vframe &frame);
void UpdateRobotDlg (net_vframe &f, int t, int i);

/***************************** CODE ******************************************/

/*
 * main _
 *
 * This is the main entry point for the gui program. This program
 * intialises the GTK gui interface and starts the gtk main process
 * it also contains the socket interactions with the soccer program
 * and the vision-radio/simulator programs depending on which is run
 *
 * RETURN VALUE: 0 on success, error code on failure
 */
int main(int argc, char *argv[])
{
  /* intialize the GTK and GDK libraries */
  gtk_set_locale();
  gtk_init(&argc, &argv);
  gdk_rgb_init();

  /* now process teh command line (after GTK has had a go) */
  char c;
  while ((c = getopt(argc, argv, "hS:V:")) != EOF) {
    switch (c) {
    case 'V': strcpy(vision_hostname, optarg); break;
    case 'S': strcpy(soccer_hostname, optarg); break;
    case 'h':
    default:
      fprintf(stderr, "\nUSAGE: gui -[hSR]\n");
      fprintf(stderr, "\t-h\tthis message\n");
      fprintf(stderr, "\t-S <host>\tWhat soccer host we should contact\n");
      fprintf(stderr, "\t-V <host>\tWhat rserver/simulator host we should contact\n");
      return (0);
    }    
  }

  // print what is the hostname
  fprintf(stderr, "Using vision host: %s\n", vision_hostname);
  fprintf(stderr, "Using soccer host: %s\n", soccer_hostname);

  /* add all the pixmap directory possibilities */
  /* probably a better way to do this */
  add_pixmap_directory("./pixmaps");
  add_pixmap_directory("./pixmaps");
  add_pixmap_directory("./gui/pixmaps");
  add_pixmap_directory("./gui/pixmaps");
  add_pixmap_directory("../gui/pixmaps");
  add_pixmap_directory("../gui/pixmaps");

  /*
   * The following code was added by Glade to create one of each component
   * (except popup menus), just so that you see something after building
   * the project. Delete any components that you don't want shown initially.
   */
  if ((mainwin = create_mainwin()) == NULL) {
    fprintf(stderr, "ERROR: Cannot create main window\n");
    exit(1);
  }
  gtk_widget_show (mainwin);

  // find the widget pointers for the mainwindow
  statusbar = lookup_widget(GTK_WIDGET(mainwin), "statusbar");
  gameinfo_entry = lookup_widget(GTK_WIDGET(mainwin), "gameinfo_entry");
  statsinfo_entry = lookup_widget(GTK_WIDGET(mainwin), "statsinfo_entry");
  tacticcombo = lookup_widget(GTK_WIDGET(mainwin), "tacticcombo");

  if ((statusbar == NULL) || (gameinfo_entry == NULL) ||
      (statsinfo_entry == NULL) || (tacticcombo == NULL)) {
    fprintf(stderr, "ERROR: Cannot find sub-windows!!!\n");
    exit(1);
  }
  
  // make the combo list
  GList *tacticlist = NULL;

  for (int i = 0; *tactic_strings[i] != '\0'; i++) {
    tacticlist = g_list_append (tacticlist, (void *) tactic_strings[i]);
  }
  tacticlist = g_list_append(tacticlist, (void *) "-------");
  for (int i = 0; *team_strings[i] != '\0'; i++) {
    tacticlist = g_list_append (tacticlist, (void *) team_strings[i]);
  }

  // now add it all to the tactics combo box
  gtk_combo_set_popdown_strings(GTK_COMBO(tacticcombo), tacticlist);

  // create the robot dialog and about dialog windows
  char winname[256];
  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      GtkWidget *w = create_robotdlg();
      GtkWidget *wbox = lookup_widget(w, "robottext");
      sprintf(winname, "Robot Dialog Team %i, ID %i", t, i);
      robotdlg[t][i].Set(w, wbox);
      gtk_window_set_title(GTK_WINDOW(w), winname);
    }
  }

  // setup the strategy debug box
  CR_SETUP(gui, STRATEGY_DISPLAY_LENGTH, CR_INT);

  GtkWidget *w = create_robotdlg();
  GtkWidget *wbox = lookup_widget(w, "robottext");
  sprintf(winname, "Strategy Dialog");
  strategydlg.Set(w, wbox, IVAR(STRATEGY_DISPLAY_LENGTH));
  gtk_window_set_title(GTK_WINDOW(w), winname);

  aboutdlg = create_aboutdlg();


  // create the refbox window but don't show it until selected
  refboxwin = create_refbox();

  /* initialize the enable table widgets */
  char wname[256];
  for (int i = 0; i < MAX_ROBOT_ID_OURS; i++) {
    sprintf(wname, "blue_robot%i", i);
    wblue[i] = lookup_widget(mainwin, wname);
    sprintf(wname, "yellow_robot%i", i);
    wyellow[i] = lookup_widget(mainwin, wname);
  }

  // initialize the ids etc
  for (int t = 0; t < NUM_TEAMS; t++) {
    covers[t] = false;
    enable[t] = false;
    for (int i = 0; i < MAX_ROBOT_ID; i++)
      enable_ids[t][i] = false;
  }

  // temp
  modeller.initialize();


  /* add the connection loop */
  // gtk_timeout_add(1000 / 2, (GtkFunction) do_connect, NULL);
  do_connect();

  // clean the field for funny X problem
  gtk_timeout_add(10*1000, (GtkFunction) do_refresh, NULL);

  /* jump to the main loop */
  gtk_main();

  running = false;

  return 0;
}

gint do_refresh(void)
{
  field.Clean();
  return 1;
}

// connect to all the servers if we have lost the connection 
// or are starting up
gint do_connect(void) 
{
  if (vision_input_id) 
    return 1;

  if (!client.IsConnected()) {
    if (client.Initialize(vision_hostname)) {
      fprintf(stderr, "Reconnected vision/radio...\n");
      UpdateStatusBar(statusbar, "ReConnected vision/radio...");
      vision_input_id = gdk_input_add(client.vision_s.get_fd(), GDK_INPUT_READ,
				      (GdkInputFunction) do_vision_recv, NULL);
    } else {
      fprintf(stderr, "Cannot reconnect vision/radio...\n");
      UpdateStatusBar(statusbar, "Cannot reconnect vision/radio...");
    }
  }
    
  /* and also connect to soccer */
  if (soccer_s.get_status() != Socket::Client) {
    if (!soccer_s.connect_client(soccer_hostname, NET_GUI_PORT)) {
      fprintf(stderr, "Cannot connect to soccer!\n");
      UpdateStatusBar(statusbar, "Cannot connected soccer...");
    } else {
      fprintf(stderr, "Connected to soccer...\n");
      UpdateStatusBar(statusbar, "Connected to soccer...");
    }
  }

  if ((!soccer_input_id) && (soccer_s.get_status() == Socket::Client)) {
    soccer_input_id = gdk_input_add(soccer_s.get_fd(), GDK_INPUT_READ,
				    (GdkInputFunction) do_soccer_recv, NULL);
  }

  return 1;
}

void do_disconnect_soccer(void)
{
  if (soccer_s.get_status() == Socket::Client)
    soccer_s.disconnect();
  if (soccer_input_id) 
    gdk_input_remove(soccer_input_id);
  soccer_input_id = 0;
}

void do_disconnect(void)
{

  if (client.vision_s.get_status() == Socket::Client) 
    client.vision_s.disconnect();
  if (vision_input_id) 
    gdk_input_remove(vision_input_id);
  vision_input_id = 0;

  // disconnect the radio as well
  if (client.radio_s.get_status() == Socket::Client) 
    client.radio_s.disconnect();

  // disconnect soccer also
  do_disconnect_soccer();

  fprintf(stderr, "Disconnected vision & radio...\n");
  UpdateStatusBar(statusbar, "Disconnected vision & radio...");
}

void do_vision_recv(void)
{
  static char msg[net_vision_out_maxsize];
  char type;

  struct net_vconfig vc;
  memset(&vc, 0, sizeof(vc));
  vc.msgtype = NET_VISION_CONFIG;

  while(client.vision_s.ready_for_recv()) {

    client.vision_s.recv_type(msg, net_vision_out_maxsize, type);

    switch(type) {
    case NET_VISION_FRAME: {
      net_vframe &new_frame = *((net_vframe *) msg);

      memcpy(&vc, &new_frame.config, sizeof(vc));

      if (selection_frozen) {
	if (field.SelectedBall()) new_frame.ball = frame.ball;
	else if (field.SelectedRobot()) {
	  char team = field.SelectedRobotTeam();
	  char id = field.SelectedRobotID();	  
	  new_frame.robots[team][id] = frame.robots[team][id];
	}
      }

      // update all the things that need to know abou tht enew frame
      for (int t = 0; t < NUM_TEAMS; t++) {
	for (int i = 0; i < MAX_TEAM_ROBOTS; i++)
	  robotdlg[t][i].Update();
      }
      strategydlg.Update();

      frame = new_frame;
      field.Update(frame);
      UpdateInfoBox(frame);
      framestats.Update(frame);

      // temp
      modeller.update(frame);

      DrawVisionInfo(frame);

//      framestats.Print();


    } break;

    default:
      fprintf(stderr, "Unknown vision message: %d.  Restarting connection!\n",
	      type);
      do_disconnect();
      break;
    }

  }

  // this needs tidying up
  if (configure) {
    do_configure(vc);

    while (!client.vision_s.ready_for_send())
      ;
    client.vision_s.send(&vc, sizeof(net_vconfig));
    configure = false;
  }
}

void do_soccer_recv(void)
{
  int rv = 1;

  while (soccer_s.ready_for_recv() && (rv > 0)) {
    char msg[net_gui_out_maxsize];
    net_gdebug *debug = (net_gdebug *) msg;
    char c;

    rv = soccer_s.recv_type(msg, net_gui_out_maxsize, c);
    if(rv <= 0){
      fprintf(stderr,"Lost contact with soccer.\n");
      do_disconnect_soccer();
      return;
    }

    // send all messages to the robot box and everything
    // else to the field manager
    if (c == NET_GUI_DEBUG_MSG) {
      if (debug->robot >= 0) {
	if (debug->level & field.GetDebugLevel()) {
	  robotdlg[debug->team][debug->robot].Add(*debug);
	}
      } else if (debug->level & field.GetDebugLevel()) {
	strategydlg.Add(*debug);
      }
    } else {
      field.Debug(*debug);
    }
  }
}


void do_configure(net_vconfig &vc)
{
  vc.msgtype = NET_VISION_CONFIG;
  
  // clear out index - id mappings and config
  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0; i < MAX_ROBOT_ID; i++) {
      map_id2index[t][i] = -1;
    }
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      map_index2id[t][i] = -1;
      vc.teams[t].robots[i].id = -1;
      vc.teams[t].robots[i].type = ROBOT_TYPE_NONE;
    }
  }

  // now generate the mappings and config structure
  for (int t = 0; t < NUM_TEAMS; t++) {
    int nr_robots = 0;
    vc.teams[t].cover_type = CHOOSE_COVER_TYPE(covers[t]);
    if (enable[t]) {
      for (int i = 0; (i < MAX_ROBOT_ID) && (nr_robots < MAX_TEAM_ROBOTS); i++) {
	if (enable_ids[t][i]) {
	  if (covers[t])
	    vc.teams[t].robots[nr_robots].type = ROBOT_TYPE_ID(i);
	  
	  vc.teams[t].robots[nr_robots].id = i;

	  // save the id-index mapping for later use
	  map_id2index[t][i] = nr_robots;
	  map_index2id[t][nr_robots] = i;
	  nr_robots++;
	}
      }
    }
  }
}

// yucky way to do it but will do for now
void do_vision_config(void)
{
  configure = true;
}  


/*
 * UpdateInfoBox -
 *
 * this function updates the information in the game control info box with
 * the stuff recieved in the net_vframe
 */
void UpdateInfoBox(net_vframe &f)
{
  char str[4096];
  char buff[256];
  net_vframe *mean, *variances, *maxframe, *acc, *maxacc, *missed;
  
  mean = framestats.GetMeanFrame();
  variances = framestats.GetVarianceFrame();
  maxframe = framestats.GetMaxFrame();
  acc = framestats.GetAccFrame();
  maxacc = framestats.GetMaxAcc();
  missed = framestats.GetMissing();
  
  // update the game info
  time_t t = (time_t) f.timestamp;
  sprintf(str, "Time: %.24s, Score %i, %i", ctime(&t), 0, 0);
  gtk_entry_set_text(GTK_ENTRY(gameinfo_entry), str);
  
  // ball info
  str[0] = 0;

  sprintf(buff, "ref cmd: %c\n", f.refstate);
  strncpy(str, buff, 4096);

  vector2f v(f.ball.state.vx, f.ball.state.vy);
  sprintf(buff, "BALL\n position (%+3.0f, %+3.0f) at speed %+3.1f\n", 
	  f.ball.state.x, f.ball.state.y, v.length());
  strcat(str,buff);

  sprintf(buff, " x-velocity %+3.1f\ty-velocity %+3.1f\n",v.x,v.y);
  strcat(str,buff);

  vector2f vv(maxframe->ball.state.vx, maxframe->ball.state.vy);
  
  sprintf(buff, " max velocity (%+3.1f, %+3.1f) = %+3.1fmm/s\n", 
	  maxframe->ball.state.vx, maxframe->ball.state.vy, vv.length());
  strcat(str,buff);

  v.set(acc->ball.state.vx, acc->ball.state.vy);
  sprintf(buff, " avg acceleration (%+3.1f, %+3.1f) = %+3.1fmm/s^2\n", 
	  acc->ball.state.vx, acc->ball.state.vy, v.length());
  strcat(str,buff);

/*
  vv.set(maxacc->ball.state.vx, maxacc->ball.state.vy);
  sprintf(buff, " max acceleration (%+3.1f, %+3.1f) = %+3.1fmm/s^2\n", maxacc->ball.state.vx, maxacc->ball.state.vy, vv.length());
  strcat(str,buff);
*/
  
  sprintf(buff, " mean confidence %1.2f\n",
	  mean->ball.vision.conf); 
  strcat(str, buff);

  //sprintf(buff," current confidence %1.2f\n", f.ball.vision.conf);
  //strcat(str,buff);
  
  sprintf(buff," lost ball percentage %2.2f\n", missed->ball.vision.conf * 100.0);
  strcat(str,buff);

  GtkSetText(statsinfo_entry, str);
  
  /* robot info */
  for (int t=0; t<NUM_TEAMS; t++) {
    for (int i=0; i<MAX_TEAM_ROBOTS; i++) {
      if (f.config.teams[t].robots[i].id >=0)
        UpdateRobotDlg(f,t,i);
    }
  }
}

void UpdateRobotDlg(net_vframe &f, int t, int i)
{
  vector2f v;
  char str[512];
  char buff[512];
  net_vframe *mean, *maxframe, *acc, *maxacc, *missed;

  mean = framestats.GetMeanFrame();
  maxframe = framestats.GetMaxFrame();
  acc = framestats.GetAccFrame();
  maxacc = framestats.GetMaxAcc();
  missed = framestats.GetMissing();

  str[0]=0;
  if (t == TEAM_BLUE)
    strcat(str, "BLUE");
  else
    strcat(str, "YELLOW");
  v.set(f.robots[t][i].state.vx, f.robots[t][i].state.vy);
  sprintf(buff, " Robot %i\n  position (%+3.0f, %+3.0f) at speed %+3.1f\n",
	  f.config.teams[t].robots[i].id, f.robots[t][i].state.x, 
	  f.robots[t][i].state.y, v.length());
  strcat(str, buff);

  sprintf(buff,"  x-velocity %+3.1f\ty-velocity %+3.1f\n",v.x,v.y);
  strcat(str,buff);

  v.set(maxframe->robots[t][i].state.vx, maxframe->robots[t][i].state.vy);

  sprintf(buff,"  max velocity (%+3.1f, %+3.1f) = %+3.1fmm/s\n", 
	  v.x, v.y, v.length());
  strcat(str,buff);

  v.set(acc->ball.state.vx, acc->ball.state.vy);
  sprintf(buff, "  avg acceleration (%+3.1f, %+3.1f) = %+3.1fmm/s^2\n", 
	  acc->ball.state.vx, acc->ball.state.vy, v.length());
  strcat(str,buff);

/*
  v.set(maxacc->ball.state.vx, maxacc->ball.state.vy);
  sprintf(buff, "  max acceleration (%+3.1f, %+3.1f) = %+3.1fmm/s^2\n", maxacc->ball.state.vx, maxacc->ball.state.vy, v.length());
  strcat(str,buff);
*/
  
  sprintf(buff, "  mean confidence %1.2f\n", mean->robots[t][i].vision.conf);
  strcat(str, buff);

  sprintf(buff," lost robot percentage %2.2f\n", missed->robots[t][i].vision.conf * 100.0);
  strcat(str,buff);
  
  robotdlg[t][i].Add(str, f.timestamp);
}


// draw all the raw vision positions
void DrawVisionInfo(net_vframe &frame)
{
  net_gdebug debug;
  
  debug.msgtype = NET_GUI_DEBUG_LINE;
  debug.robot = -1;
  debug.level = GDBG_TRACKER;
  vector2d p = vftod(frame.ball.vision.pos);
  vector2d v(frame.ball.state.vx, frame.ball.state.vy);
  vdtostruct(p, debug.info.line.p[0]);
  vdtostruct(p + v, debug.info.line.p[1]);
  debug.info.line.flags = G_ARROW_FORW;
  field.Debug(debug);

  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0;i < MAX_TEAM_ROBOTS; i++) {
      if ((debug.robot = frame.config.teams[t].robots[i].id) >= 0) {
	vector2d p = vftod(frame.robots[t][i].vision.pos);
	vector2d v(100.0, 0.0);
	v = v.rotate(frame.robots[t][i].vision.angle);
	
	vdtostruct(p, debug.info.line.p[0]);
	vdtostruct(p + v, debug.info.line.p[1]);
	debug.level = GDBG_VISION;
	debug.info.line.flags = G_ARROW_FORW;
	field.Debug(debug);
      }
    }
  }
}
